#!/bin/bash
shopt -s expand_aliases

# for env

# for env

source ${PX4_WS}/Tools/setup_gazebo.bash ${PX4_WS} ${PX4_WS}/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:${PX4_WS}
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:${PX4_WS}/Tools/sitl_gazebo

MULTI_UAV_LAUNCH_FILE=multi_uav_livox.launch

function ChangeWorld() {
    if [[ "$#" -eq 1 ]];then
        if [[ -f "$1" ]];then
            cp "$1" "${PX4_WS}/Tools/sitl_gazebo/worlds/main.world"

            sed -i "8 c \<arg name=\"world\" default=\"\$\(find mavlink_sitl_gazebo\)\/worlds\/main.world\"\/\>" ${PX4_WS}/launch/${MULTI_UAV_LAUNCH_FILE}
            echo "change world"
        fi
    fi
}

if [[ -f "multi_uav.launch" ]];then
    cp multi_uav.launch ${PX4_WS}/launch/multi_uav.launch
    MULTI_UAV_LAUNCH_FILE=multi_uav.launch
fi

if [[ "$#" -eq 0 ]];then
    roslaunch px4 ${MULTI_UAV_LAUNCH_FILE}
elif [[ "$#" -eq 1 ]];then
    if [[ -f "$1.world" ]];then
        ChangeWorld "$1.world"
    else
        wget "${WORLD_REPO}/$1.world"
        if [[ -f "$1.world" ]];then
            ChangeWorld "$1.world"
        else
            echo "file $1.world is not exist"
        fi
        rm -rf "${direction_name}"
    fi

    roslaunch px4 ${MULTI_UAV_LAUNCH_FILE}
else
    echo "input error"
    echo "e.g. \"bash \${PX4_WS}/gazebo_run world_directory\" "
fi
